#include <caros/hand_detection/leap_hand_processing.h>

#include <Leap.h>
#include <LeapMath.h>

LeapHandProcess::LeapHandProcess()
{
}

Leap::Vector LeapHandProcess::processHandPosition(const Leap::Hand& hand)
{
  Leap::Vector res = hand.palmPosition();
  res.x /= 1000;
  res.y /= 1000;
  res.z /= 1000;
  return res;  // Output in meters instead of millimeters
}
Quaternion LeapHandProcess::processHandRotation(const Leap::Hand& hand)
{
  Leap::Vector dirF = hand.direction();
  Leap::Vector dirD = hand.palmNormal();
  Leap::Vector dirS = dirF.cross(dirD);
  dirS /= dirS.magnitude();

  Rotation3D rot;

  for (unsigned int i = 0; i < 3; i++)
  {
    rot[i][0] = dirD[i];
    rot[i][1] = dirF[i];
    rot[i][2] = dirS[i];
  }

  return Quaternion(rot);
}

double LeapHandProcess::processGrab(const Leap::Hand& hand)
{
  return hand.grabStrength();
}

double LeapHandProcess::processSphereGrab(const Leap::Hand& hand){
  return hand.sphereRadius();
}

void LeapHandProcess::setStartFrame(const Leap::Frame& frame)
{
  start_frame_ = frame;
}

Quaternion::Quaternion(const Rotation3D& rot)
{
  // The method
  const double min = -0.9;
  const double min1 = min / 3.0;

  const double tr = rot[0][0] + rot[1][1] + rot[2][2];

  if (tr > min)
  {
    const double s = 0.5 / sqrt(tr + 1.0);
    w = (0.25) / s;
    x = (rot[2][1] - rot[1][2]) * s;
    y = (rot[0][2] - rot[2][0]) * s;
    z = (rot[1][0] - rot[0][1]) * s;
  }
  else
  {
    if (rot[0][0] > min1)
    {
      const double sa = (sqrt(rot[0][0] - rot[1][1] - rot[2][2] + 1.0));
      x = 0.5 * sa;
      const double s = (0.25) / x;
      y = (rot[0][1] + rot[1][0]) * s;
      z = (rot[0][2] + rot[2][0]) * s;
      w = (rot[2][1] - rot[1][2]) * s;
    }
    else if (rot[1][1] > min1)
    {
      const double sb = (sqrt(rot[1][1] - rot[2][2] - rot[0][0] + 1));
      y = (0.5) * sb;

      const double s = (0.25) / y;
      x = (rot[0][1] + rot[1][0]) * s;
      z = (rot[1][2] + rot[2][1]) * s;
      w = (rot[0][2] - rot[2][0]) * s;
    }
    else
    {
      const double sc = (sqrt(rot[2][2] - rot[0][0] - rot[1][1] + 1));
      z = (0.5) * sc;

      const double s = (0.25) / z;
      x = (rot[0][2] + rot[2][0]) * s;
      y = (rot[1][2] + rot[2][1]) * s;
      w = (rot[1][0] - rot[0][1]) * s;
    }
  }
}